#include "SceneManager.h"

namespace RTRT
{
    template<> SceneManager* Singleton<SceneManager> :: ms_singleton = 0;
    
    SceneManager :: SceneManager() : 
        m_sceneObjectList(NULL),
        m_rootNode(NULL),
        m_sceneObjCount(0)
    {
        
    }
    
    SceneManager :: ~SceneManager()
    {
        
    }
    
    int SceneManager ::initialise()
    {
        // Add geometry objects
        
        // Set renderer
        
        // Build KD-Tree
		// @TODO: Make this configurable
        Vector3 p1 = Vector3(-14,-6,-6);
        Vector3 p2 = Vector3(14,8,30);
        m_aabb = AxisAlignedBox(p1,p2-p1);
		m_rootNode = createKDTreeNode();
        
        return 1;
    }
    
	KDTreeNode *SceneManager::createKDTreeNodePairImpl() 
	{
		/*
		KDTreeNode *pair = new KDTreeNode[2];
		m_nodeList.add(pair[0]);
		m_nodeList.add(pair[1]);
		*/
		
		return MemoryPool::getSingleton().newKDTreeNodePair();
	}
	
    KDTreeNode *SceneManager::createKDTreeNodePair()
    {
		return createKDTreeNodePairImpl();
    }
    
	void SceneManager::insertSplitPlane(const float splitPos)
	{
		// Allocate a list from the pool
		SplitList *entry = m_splitPool;
		m_splitPool = m_splitPool->next;
		entry->next = 0;
		entry->splitPosition = splitPos;
		entry->n1count = entry->n2count = 0; 
		if (NULL == m_splitList) 
			m_splitList = entry;
		else
		{
			// Insert the entry as the first(smallest) item in the list
			if (splitPos < m_splitList->splitPosition)
			{
				entry->next = m_splitList;
				m_splitList = entry;
			}
			else if (splitPos == m_splitList->splitPosition)
			{
				entry->next = m_splitPool;
				m_splitPool = entry;
			}
			else 
			{
				SplitList *tList = m_splitList;
				while (tList->next) && (splitPos >= tList->next->splitPosition)
				{
					if (splitPos == tList->next->splitPosition)
					{
						entry->next = m_splitPool;
						m_splitPool = entry;
					}
					tList = tList->next;
				}
				entry->next = tList->next;
				tList->next = entry;
			}
		}
	}
	
    KDTreeNode *SceneManager::buildKDTree()
    {
        // Attach all the scene objects to the root node
        m_rootNode->attachObject();
        
        m_splitPool = new SplitList[m_sceneObjCount * 2 + 8];
        int i = 0;
        for (; i < (m_sceneObjCount * 2 + 6); i++)
        {
            m_splitPool[i].next = &m_splitPool[i + 1];
        }
        m_splitPool[i].next = 0;
        m_splitList = NULL;
        
        subDivide(m_rootNode,m_aabb,0,m_sceneObjCount);
        
    }
    
    void subDivide(const KTTreeNode *curNode, const AxisAlignedBox& curNodeBox, int depth, size_t triCount)
    {
        // Recycle used split list nodes
        if (m_splitList)
        {
            SplitList* list = m_splitList;
            while (list->next)	
                list = list->next;
            list->next = m_splitPool;
            m_splitPool = m_splitList, m_splitList = 0;
        }
        
        // Determine split axis
        Vector3 size = curNodeBox.getSize();
        if( ( size.x >= size.y ) && (size.x >= size.z) )
            curNode->setAxis(0);
        else if( (size.y >= size.x) && (size.y >= size.z) )
            curNode->setAxis(1);
        
		int axis = curNode->getAxis();
		
		// Make a list of the split position candidates
		SceneObject *objects = curNode->getSceneObjects();
		float splitPos;
		float minAABB = curNodeBox.getPosition().cell(axis);
		float maxAABB = curNodeBox.getPosition().cell(axis) + curNodeBox.getSize().cell(axis);
		
		bool *pRight = new bool[triCount];
		float *eLeft = new float[triCount];
		float *eRight = new float[triCount];
		
		Triangle **triArray = new Triangle*[triCount];
		
		int index = 0;
		while (objects) 
		{
			Triangle *pTri = triArray[index] = objects->getTrianlgMesh();
			pRight[index] = true;
			// Calculate the spatial range covered by this triangle in a certain axis
			pTri->calculateRange(eLeft[index], eRight[index], axis);
			index++;
			for (int i = 0; i < 3; i++)
			{
				splitPos = pTri->getVertex(i).cell(axis);
				if (fPos >= minAABB && fPos <= maxAABB)
					insertSplitPlane(splitPos);
			}
			objects = objects->next();
		}
		
		// Calculate n1count/n2count for each split position
		AxisAlignedBox b1, b2, b3, b4 = curNodeBox;
		SplitList *splitListIterator = m_splitList;
		float b3p1 = b3.getPosition().cell(axis);
		float b4p2 = b4.getPosition().cell(axis) + b4.getSize().cell(axis);
		while (splitList)
		{
			b4.getPosition().cell(axis);
			b4.getSize().cell(axis) = maxAABB - splitListIterator->splitPostion;
			b3.getSize().cell(axis) = splitListIterator->splitPosition - minAABB;
			
			float b3p2 = b3.getPosition().cell(axis) + b3.getSize().cell(axis);
			float b4p1 = b4.getPosition().cell(axis);
			
			for (int i = 0; i < triCount; i++)
			{
				if (pRight)
				{
					Triangle *tri = triArray[i];
					if ( (eLeft[i] <= b3p2) && (eRight[i] >= b3p1) )
						if (tri->intersectBox(b3))
							splitListIterator->n1count++;
					if ( (eLeft[i] <= b4p2) && (eRight[i] >= b4p1))
						if (tri->intersectBox(b4))
							splitListIterator->n2count++;
						else
							pRight[i] = false;
				}
				else 
					splitListIterator->n1count++;
			}
			splitListIterator = splitListIterator->next;
		}
		delete pRight;
		// Calculate the surface area of current node
		float saCurNodeInv = 0.5/(curNode.getWidth()*curNode.getHeight() +
		 				curNode.getHeight()*curNode.getDepth() + 
						curNode.getDepth()*curNode.getWidth());
						
		// Calculate the cost for not splitting
		float noSplitCost = triCount * 1.0f;
		splitListIterator = m_splitList;
		float lowCost = 10000;
		float bestPosSplit = 0;
		while (splitListIterator)
		{
			// Calculate child nodes bounding box
			b4.getPosition().cell(axis) = splitListIterator->splitPosition;
			b4.getSize().cell(axis) = maxAABB - splitListIterator->splitPosition;
			b3.getSize().cell(axis) = splitListIterator->splitPosition - minAABB;
			
			// Calculate child node cost
			float saLeft = 2 * (b3.getWidth()*b3.getHeight() +
			 				b3.getHeight()*b3.getDepth() + 
							b3.getDepth()*b3.getWidth());
			float saRight = 2 * (b4.getWidth()*b4.getHeight() +
						 		b4.getHeight()*b4.getDepth() + 
								b4.getDepth()*b4.getWidth());
			float splitCost = 0.3f + 1.0f * (saLeft*saCurNodeInv*splitListIterator->n1count + 
											saRight*saCurNodeInv*splitListIterator->n2count);
			
			// Update the best cost
			if (splitCost < lowCost)
			{
				lowCost = splitCost;
				bestCostSplit = splitListIterator->splitPosition;
				b1 = b3; b2 = b4;
			}
			splitListIterator = splitListIterator->next;								
		}
		if (lowCost > noSplitCost)
			return;
		curNode->setSplitPos(bestCostSplit);
		
		// Create new child nodes pair
		KDTreeNode *leftChild = createKDTreeNodePair();
		int n1Count = 0, n2Count = 0, total = 0;
		// attach primitives to both side
		float b1p1 = b1.GetPos().cell(axis);
		float b2p2 = b2.GetPos().cell(axis) + b2.GetSize().cell(axis);
		float b1p2 = b1.GetPos().cell(axis) + b1.GetSize().cell(axis);
		float b2p1 = b2.GetPos().cell(axis);
		
		for (int i=0; i<triCount; i++)
		{
			Triangle *tri = triArray[i];
			total++;
			if ((eLeft[i] <= b1p2) && (eRight[i] >= b1p1)) 
				if (p->intersectBox(b1)) 
				{
					leftChild->addGeometry(p);
					n1Count++;
				}
			if ((eLeft[i] <= b2p2) && (eRight[i] >= b2p1)) 
				if (p->intersectBox(b2)) 
				{
					(leftChild+1)->addGeometry(p);
					n2Count++;
				}
		}
		
		delete eLeft;
		delete eRight;
		delete triArray;
		
		MemoryPool::getSingleton().freeTriListEntry(curNode->getGeometryList());
		curNode->setLeftChild(leftChild);
		curNode->setLeaf(false);
		
		if (depth < MAX_TREE_DEPTH)
		{
			if (n1Count > 2) subDivide(left, b1, depth+1, n1Count);
			if (n2Count > 2) subDivide(left+1, b2, depth+2, n2Count);
		}
    } // end of subDivide()
	
	void SceneManager::addTriangleList(const Triangle *triList, const size_t triCount)
	{
		for (int i=0;i<triCount;i++)
			m_triangleArray.add(triList);
	}
}
